import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    p2p_move_base_dir = get_package_share_directory('p2p_move_base')
    config_file_path = os.path.join(p2p_move_base_dir, 'config', 'B2_localization.yaml')
    rviz_config_path = os.path.join(p2p_move_base_dir, 'rviz', 'p2p_move_base_localization.rviz')

    use_sim_arg = DeclareLaunchArgument(
        'use_sim',
        default_value='true',
        description='Use simulation time if true'
    )

    declare_points_topic_cmd = DeclareLaunchArgument(
        'points_topic', 
        default_value='/rslidar_points_synced', 
        description='LiDAR topic'
    )
        
    declare_imu_topic_cmd = DeclareLaunchArgument(
        'imu_topic', 
        default_value='/dog_imu_raw_synced', 
        description='IMU topic'
    )
        
    declare_globalmap_pcd_cmd = DeclareLaunchArgument(
        'globalmap_pcd',
        default_value='src/hdl_Localization_ROS2/hdl_localization/data/DAM-563-Original-merge-lite-5cm-ascii.pcd',
        description='Full path to 3D PCD map file'
    )
    
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace', default_value='', description='Top-level namespace'
    )

    use_sim = LaunchConfiguration('use_sim')
    points_topic = LaunchConfiguration('points_topic')
    imu_topic = LaunchConfiguration('imu_topic')
    globalmap_pcd = LaunchConfiguration('globalmap_pcd')
    namespace = LaunchConfiguration('namespace')


    load_hdl_localization = ComposableNodeContainer(
        name='hdl_localization_container',
        namespace=namespace,
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='hdl_localization',
                plugin='hdl_localization::GlobalmapServerNodelet',
                name='GlobalmapServerNodelet',
                parameters=[
                    {'use_sim_time': use_sim},
                    {'globalmap_pcd': globalmap_pcd},
                    {'downsample_resolution': 0.2},
                    {'convert_utm_to_local': True},
                ],
            ),
            ComposableNode(
                package='hdl_localization',
                plugin='hdl_localization::HdlLocalizationNodelet',
                name='HdlLocalizationNodelet',
                remappings=[
                    ('/ouster/points', points_topic),
                    ('/gpsimu_driver/imu_data', imu_topic),
                    ('initialpose', '/initial_3d_pose'), 
                ],
                parameters=[
                    {'use_sim_time': use_sim},
                    {'robot_odom_frame_id': 'odom'},
                    {'odom_child_frame_id': 'base_link'},
                    {'enable_robot_odometry_prediction': False}, 
                    {'use_imu': True},
                    {'ndt_resolution': 0.5},
                    {'downsample_resolution': 0.2},
                    {'use_global_localization': False},
                    {'specify_init_pose': False},
                    {'ndt_maximum_iterations': 50},
                    {'map_intensity_threshold': 23.0}, 
                    {'ndt_neighbor_search_method': 'KDTREE'},
                    {'ndt_neighbor_search_radius': 3.0},
                    {'cool_time_duration': 1.0},
                ],
            ),
        ],
        output='screen',
        emulate_tty=True,
    )

    node_floor_filter = Node(
        package='p2p_move_base',
        executable='floor_filter_node',
        name='floor_filter_node',
        output='screen',
        parameters=[
            {'use_sim_time': use_sim},
            {'min_z': -1.0},       
            {'max_z': 1.0},
            
            {'normal_search_radius': 0.5},
            
            {'max_slope_angle': 30.0}, 

            {'leaf_size': 0.2}
        ],
        remappings=[
            ('input_cloud', '/globalmap_filtered'),
            ('output_ground', '/mapground')
        ]
    )

    node_global_planner = Node(
        package='global_planner',
        executable='global_planner_node',
        output='screen',
        parameters=[
            config_file_path,
            {'use_sim_time': use_sim}
        ]
    )

    node_p2p_move_base = Node(
        package='p2p_move_base',
        executable='p2p_move_base_node',
        output='screen',
        parameters=[
            config_file_path,
            {'use_sim_time': use_sim}
        ]
    )

    node_clicked2goal = Node(
        package='p2p_move_base',
        executable='clicked2goal.py',
        output='screen',
        parameters=[
            {'use_sim_time': use_sim}
        ]
    )

    node_rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        output='screen',
        arguments=['-d', rviz_config_path],
        parameters=[
            {'use_sim_time': use_sim}
        ]
    )

    return LaunchDescription([
        use_sim_arg,
        declare_namespace_cmd,
        declare_points_topic_cmd,
        declare_imu_topic_cmd,
        declare_globalmap_pcd_cmd,
        
        load_hdl_localization,
        node_floor_filter,
        node_global_planner,
        node_p2p_move_base,
        node_clicked2goal,
        node_rviz2
    ])